#include <math.h>
#include <unistd.h>
#include <sys/socket.h>
#include "nvs_flash.h"
#include "esp_log.h"
#include "mpu6050.h"
#include "motor_control/controller.h"
#include "data_type.h"
#include "wifi_station.h"
#include "wireless_comm.h"
#include "driver/uart.h"
#include "cJSON.h"

#define PLUSE_PER_ROUND     50 * 7 * 2
#define WHEEL_SPACE         6.7e-2
#define WHEEL_PERIMETER     M_PI * 4.4e-2 * 17.5 // m

#define PID_P   1.7
#define PID_I   1.2
#define PID_D   0.01

static void mc_init(motor_control_context_t *ctx) {
    mc_timer_loop_create(100, 100);
    mc_config_t config = MC_CONFIG_DEFAULT(PID_P, PID_I, PID_D);
    config.encoder_config.round_count = PLUSE_PER_ROUND;
    config.motor_config.pwma_gpio_num = GPIO_NUM_36;
    config.motor_config.pwmb_gpio_num = GPIO_NUM_35;
    config.encoder_config.edge_gpio_num = GPIO_NUM_19;
    config.encoder_config.level_gpio_num = GPIO_NUM_20;
    ESP_ERROR_CHECK(mc_new(&config, 150, &ctx->left));
    ESP_ERROR_CHECK(mc_add_to_timer_loop(ctx->left));
    config.motor_config.pwma_gpio_num = GPIO_NUM_34;
    config.motor_config.pwmb_gpio_num = GPIO_NUM_33;
    config.encoder_config.edge_gpio_num = GPIO_NUM_14;
    config.encoder_config.level_gpio_num = GPIO_NUM_12;
    ESP_ERROR_CHECK(mc_new(&config, 150, &ctx->right));
    ESP_ERROR_CHECK(mc_add_to_timer_loop(ctx->right));
    mc_timer_loop_start();
}

static void i2c_bus_init(i2c_port_t port, gpio_num_t sda, gpio_num_t scl) {
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = sda,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_io_num = scl,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = 100000,
        .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL,
    };
    ESP_ERROR_CHECK(i2c_param_config(port, &conf));
    ESP_ERROR_CHECK(i2c_driver_install(port, conf.mode, 0, 0, 0));
}

static void i2c_sensor_mpu6050_init(mpu6050_context_t *ctx) {
    static const i2c_port_t port = I2C_NUM_0;
    i2c_bus_init(port, GPIO_NUM_37, GPIO_NUM_38);
    ctx->sensor = mpu6050_create(port, MPU6050_I2C_ADDRESS);
    ESP_ERROR_CHECK(ctx->sensor != NULL ? ESP_OK : ESP_ERR_NO_MEM);
    ESP_ERROR_CHECK(mpu6050_config(ctx->sensor, ACCE_FS_4G, GYRO_FS_1000DPS));
    ESP_ERROR_CHECK(mpu6050_wake_up(ctx->sensor));
    uint8_t deviceid;
    ESP_ERROR_CHECK(mpu6050_get_deviceid(ctx->sensor, &deviceid));
    ESP_LOGI("mpu6050", "device id: 0x%X", deviceid);
    ctx->acce_cal.raw_acce_x = 460;
    ctx->acce_cal.raw_acce_y = 83;
    ctx->acce_cal.raw_acce_z = 7373;
    ctx->gyro_cal.raw_gyro_x = -69;
    ctx->gyro_cal.raw_gyro_y = 57;
    ctx->gyro_cal.raw_gyro_z = -16;
}

static void sensor_mpu6050_sample_task(void *pvParameters) {
    mpu6050_context_t *ctx = pvParameters;
    for (;;) {
        if (mpu6050_get_raw_acce(ctx->sensor, &ctx->curr_acce) != ESP_OK) {
            ESP_LOGW(__func__, "update acce failed");
        }
        if (mpu6050_get_raw_gyro(ctx->sensor, &ctx->curr_gyro) != ESP_OK) {
            ESP_LOGW(__func__, "update gyro failed");
        }
        vTaskDelay(pdMS_TO_TICKS(50));
    }
}

void app_main(void) {
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);

    static context_pack_t ctx = {
        .wheel_space = WHEEL_SPACE,
        .wheel_perimeter = WHEEL_PERIMETER,
    };
    mc_init(&ctx.mc);
    i2c_sensor_mpu6050_init(&ctx.mpu6050);
    wifi_connect_sta("Xiaomi 14 Pro", "1048794538", WIFI_AUTH_WPA_WPA2_PSK);
    start_mdns_service("chassis", "smart car chassis control");
    xTaskCreate(sensor_mpu6050_sample_task, "mpu6050_sample",
        4096, &ctx.mpu6050, 4, NULL);
    start_wireless_comm(&ctx);

    char buffer[128];
    int sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
    struct sockaddr_in addr = {
        .sin_family = AF_INET,
        .sin_addr.s_addr = IPADDR_ANY,
        .sin_port = htons(1000),
    };
    (void)bind(sock, (struct sockaddr *)&addr, sizeof(addr));
    listen(sock, 1);
    for (;;) {
        int c = accept(sock, NULL, NULL);
        for (;;) {
            ssize_t len = recv(c, buffer, sizeof(buffer), 0);
            if (len > 0) {
                buffer[len] = 0;
                cJSON *root = cJSON_Parse(buffer);
                if (root && cJSON_IsArray(root) && cJSON_GetArraySize(root) >= 3) {
                    float p = cJSON_GetArrayItem(root, 0)->valuedouble;
                    float i = cJSON_GetArrayItem(root, 1)->valuedouble;
                    float d = cJSON_GetArrayItem(root, 2)->valuedouble;
                    mc_set_pid_params(ctx.mc.left, p, i, d);
                    mc_set_pid_params(ctx.mc.right, p, i, d);
                    ESP_LOGI(__func__, "successfully set pid");
                }
                cJSON_Delete(root);
            }
        }
    }
}
